#ifndef _avoidance_H_
#define _avoidance_H_

#include "reg52.h"
#include <intrins.h>
#include "pwm.h"

//fix:参数还没调
#define turnTime 1000 //旋转时长
#define avoidTime 2000 //转完之后走的时长
#define forwardTime 4000 //离开障碍物之后向前走绕开的时长

void delay_nus( u16 i ) /* 延时:i>=12 ,i的最小延时单12 us */
{
	i = i / 10;
	while ( --i );
}


void delay_nms( u16 n ) /* 延时n ms */
{
	n = n + 1;
	while ( --n )
		delay_nus( 900 ); /* 延时 1ms,同时进行补偿 */
}

u8 nowleftV;
u8 nowRightV;

void moveTime(u16 time)
{
    setV(nowleftV,nowRightV);
    delay_nms(time);
    stop();
    delay_nms( 300 ); //停止300MS   防止电机反相电压冲击  导致系统复位
}

void avoidance()
{
    nowleftV=V_left;
    nowRightV=V_right;

    stop();
    delay_nms( 300 ); //停止300MS   防止电机反相电压冲击  导致系统复位
    //后退，其实这个可以没有
    setDir(0,0);
    moveTime(1000);
    //转弯
    setDir(1,0);
    moveTime(turnTime);
    //为了避障前进
    setDir(1,1);
    moveTime(avoidTime);
    //转回去
    setDir(0,1);
    moveTime(turnTime);
    //在障碍物外前进
    setDir(1,1);
    moveTime(forwardTime);
    //已经绕开障碍物，转回去
    setDir(0,1);
    moveTime(turnTime);
    //回到原先位置
    setDir(1,1);
    moveTime(avoidTime);
    //转正
    setDir(1,0);
    moveTime(turnTime);
    setV(nowleftV,nowRightV); //之后还得接着走
}

#endif